#include <cereal/archives/json.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/unordered_map.hpp>
#include <fstream>
#include <obstacle.hpp>
#include <random>

std::random_device rd;
std::mt19937 eng(rd());
std::uniform_real_distribution<float> distr(0.0, 2.0f);
Vec3f random_vec3f() { return Vec3f{distr(eng), distr(eng), distr(eng)}; }
Quat4f random_quat4f() {
  return Quat4f{distr(eng), distr(eng), distr(eng), distr(eng)};
}
Pose3f random_pose3f() {
  return Pose3f{
      .position{random_vec3f()},
      .orientation{random_quat4f()},
  };
}

Trajectory random_trajectory() {
  Trajectory trajectory;
  trajectory.set_reference_frame(random_pose3f());
  auto &poses = trajectory.get_poses_mutable();
  auto &timestamps = trajectory.get_timestamps_mutable();
  for (int i = 0; i < 5; ++i) {
    poses.emplace_back(random_pose3f());
    timestamps.emplace_back(i + 1999);
  }
  return trajectory;
}

int main() {

  Obstacle obstacle(ObstacleType::Automobile, 1231, random_trajectory());
  std::ofstream os("out.json", std::ios::binary);
  cereal::JSONOutputArchive archive(os);
  archive(obstacle);

  return 0;
}